#include "bessel.h"
#include <math.h>

void   BesselTrajectory(float t, GaitParams params, float gaitOffset, float &x, float &y)/*获得足端坐标*/
{
    static float p = 0;
    static float prev_t = 0;

    float stanceHeight = params.stance_height;
    float downAMP = params.down_amp;
    float upAMP = params.up_amp;
    float flightPercent = params.flight_percent;
    float stepLength = params.step_length;
    float FREQ = params.freq;


 //   p += FREQ * (t - prev_t < 50.0f ? t - prev_t : 0); // 记录步态周期百分比进度
 //   prev_t = t;

     float  gp = fmod((t+gaitOffset),1.0f); // 步态周期进度+腿相位

	  /*飞行相*/
    if (gp <= flightPercent)
	{
         float percentFlight = gp/flightPercent;
         x = (gp/flightPercent)*stepLength - stepLength/2.0f;
         y = -2*upAMP*(percentFlight-pow(percentFlight,2)) + stanceHeight;
    }
		/*支撑相*/
    else
	{
        float percentBack = (gp-flightPercent)/(1.0f-flightPercent);
        x = -percentBack*stepLength + stepLength/2.0f;
        y = 2*downAMP*(percentBack-pow(percentBack,2)) + stanceHeight;
    }
}